"""
Create: JiBingyu 2024-07-15
Description: 该文件是算法的主文件，用于调用算法库中的算法实现，实现多艇编队控制。
"""

import ctypes
import os
import threading
from multiprocessing import Lock, Process
from typing import List

import matplotlib.patches as patches
import matplotlib.pyplot as plt
import numpy as np

from cbs_mapf.sim import *
from Function_j import *
from MultiUSVModel import MultiUSVModel
from StructType import *
from test_AF import USVLOSController

# 如果没有dataRecord文件夹就生成一个
if not os.path.exists("dataRecord"):
    os.makedirs("dataRecord")
# 全局变量定义
Task_data = []  # 任务点， taskPath_list并不是每次都有，所以需要一个全局变量来存储
Refe_data = []  # 参考点， referInfo_list并不是每次都有，所以需要一个全局变量来存储
Obs_data = []  # 障碍物，其中obsR和fOBSV与说明书上相反，代码中半径为0速度为200，说明书上半径为200速度为0
USV_data = []  # 实时的USV位置信息
GOAL = []
START = []
targer_location = []  # 目标位置
plotting = True  # 是否绘图
clean_flag = True  # 不需要保存数据，方便Push代码
# 各艇控制指令
multiUSVInitInfo: List[USVInitStructType] = []
time_time = 0  # 记录运行时间，存放在Time.txt中
pathnum = 1
# source_dir = "test/test" + str(pathnum)
source_dir = "test/conflict"
current_target_index = []
# Path_usv = np.load("path_dir/path" + str(pathnum) + ".npy", allow_pickle=True).tolist()
Path_usv = np.load("path_dir/conflict_path.npy", allow_pickle=True).tolist()
# for i in range(len(Path_usv)):
#     Path_usv[i] = Path_usv[i][0:20]
if len(Path_usv[0]) < 80:
    # 用最后一个数据填充
    for i in range(len(Path_usv)):
        last_value = Path_usv[i][-1]
        num_to_add = 80 - len(Path_usv[i])
        padding = np.array(
            [[last_value[0] + 20 * j, last_value[1]] for j in range(1, num_to_add + 1)]
        )
        Path_usv[i] = np.concatenate((Path_usv[i], padding), axis=0)

# LOS + PD控制器参数
delta = 1.0
kp = 0.5
kd = 0.1
max_angular_velocity = 1.0
max_angular_acceleration = 0.1

copy_files(source_dir, "config")
clear_dataRecord_folder()


def algorithmImpl(
    algIterNum,
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
    controller,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    global targer_location
    global current_target_index
    global Path_usv
    global START, GOAL
    multiUSVStatus_list = Get_USV_data(
        taskPath_data,
        taskPath_size,
        referInfo_data,
        referInfo_size,
        curOBSFusion_data,
        curOBSFusion_size,
        multiUSVStatus_data,
        multiUSVStatus_size,
    )
    # agents_list = min_cost(START, GOAL)
    # agents_list = np.load(
    #     "path_dir/agents_list" + str(pathnum) + ".npy", allow_pickle=True
    # ).tolist()
    agents_list = np.load(
        "path_dir/conflict_agents_list.npy", allow_pickle=True
    ).tolist()
    #################################### 航点优化部分 ###############################

    # 初始化目标位置
    if len(targer_location) == 0:
        targer_location = np.zeros((5, len(Refe_data), 2))
        current_target_index = np.zeros(len(Refe_data), dtype=int)
    # 参考ID和参考点的XY坐标,选择主舰的航点
    for id, info in enumerate(agents_list):
        if Refe_data[info][-1] == 1:
            refer_ID = id
    refer_location = [
        [
            Go_to_xy2(
                multiUSVStatus_list[refer_ID].dbCurX,
                multiUSVStatus_list[refer_ID].dbCurY,
                Task_data[-1][0],
                Task_data[-1][1],
            ),
            [
                multiUSVStatus_list[refer_ID].dbCurX,
                multiUSVStatus_list[refer_ID].dbCurY,
            ],
        ]
    ][0]

    multiUSVInitInfo: List[USVInitStructType] = []
    for i, it in enumerate(multiUSVStatus_list):
        updateUSVControl = USVInitStructType()
        updateUSVControl.iUSVId = it.iUSVId
        updateUSVControl.dbInitX = it.dbCurX
        updateUSVControl.dbInitY = it.dbCurY

        #################################### 控制算法部分 ###############################

        (
            targer_location[0][i],
            targer_location[1][i],
            yaw,
        ) = Algorithm_cbs(
            i,
            multiUSVStatus_list,
            refer_location,
            Refe_data,
            [agents_list[i]],
            Path_usv[i],
            controller[i],
            1,
        )
        low_speed = 3
        find_speed = 8
        full_speed = 14
        dert_speed = 2
        vel_ass = multiUSVStatus_list[i].fCurVel
        for indsx, info in enumerate(multiUSVStatus_list):
            if indsx == i:
                continue
            else:
                distance_ = np.sqrt(
                    (info.dbCurX - multiUSVStatus_list[i].dbCurX) ** 2
                    + (info.dbCurY - multiUSVStatus_list[i].dbCurY) ** 2
                )
                if distance_ < 50:
                    print("碰撞：", i, indsx)
                    return
                if distance_ < 150:
                    if info.dbCurX <= multiUSVStatus_list[i].dbCurX:
                        vel_ass = min(full_speed, vel_ass + dert_speed)
                    else:
                        vel_ass = max(low_speed, vel_ass - dert_speed)
                    # break
        updateUSVControl.fInitVel, updateUSVControl.fInitYaw = vel_ass, yaw
        multiUSVInitInfo.append(updateUSVControl)
    vel_ass, yaw_ass = adjust_ass(
        multiUSVStatus_list, targer_location[0], low_speed, find_speed, full_speed
    )
    if vel_ass != []:
        for idx, info in enumerate(multiUSVInitInfo):
            info.fInitVel = vel_ass[idx]
            info.fInitYaw = yaw_ass[idx]
    return multiUSVInitInfo
    ##################################################################################


def Get_USV_data(
    taskPath_data,  # 编队航线
    taskPath_size,
    referInfo_data,  # 编队构型
    referInfo_size,
    curOBSFusion_data,  # 障碍物信息
    curOBSFusion_size,
    multiUSVStatus_data,  # 编队内艇信息
    multiUSVStatus_size,
):
    global Task_data
    global Refe_data
    global Obs_data
    global USV_data
    global GOAL
    global START
    taskPath_list: List[SFormationTaskInfo] = [
        taskPath_data[index] for index in range(taskPath_size.value)
    ]
    referInfo_list: List[referUSVInfo] = [
        referInfo_data[index] for index in range(referInfo_size.value)
    ]
    curOBSFusion_list: List[InputOBSStruct] = [
        curOBSFusion_data[index] for index in range(curOBSFusion_size.value)
    ]
    multiUSVStatus_list: List[USVOutPutStructType] = [
        multiUSVStatus_data[index] for index in range(multiUSVStatus_size.value)
    ]
    Task_data = [] if len(taskPath_list) != 0 else Task_data
    Refe_data = [] if len(referInfo_list) != 0 else Refe_data
    Obs_data = [] if len(curOBSFusion_list) != 0 else Obs_data
    USV_data = [] if len(multiUSVStatus_list) != 0 else USV_data
    for it in taskPath_list:
        Task_data.append([it.referX, it.referY])
    for it in referInfo_list:
        Refe_data.append([it.relatDis, it.relatYaw, it.USVID, it.type])
    for it in curOBSFusion_list:
        # 这里的 obsR 和 fOBSV与说明书上反的
        Obs_data.append([it.dOBSX, it.dOBSY, it.obsR])
    for it in multiUSVStatus_list:
        USV_data.append([it.dbCurX, it.dbCurY])
    if len(GOAL) == 0:
        for item in USV_data:
            START.append((200 + int(item[0]), 400 + int(item[1])))
        for item in Refe_data:
            GOAL.append(
                [
                    int(
                        4000 - 2000 + round(item[0] * np.cos(np.deg2rad(90 - item[1])))
                    ),
                    int(0 + 400 + round(item[0] * np.sin(np.deg2rad(90 - item[1])))),
                ]
            )
    return multiUSVStatus_list


def plot_graph():
    global plotting
    global Task_data
    global Obs_data
    global USV_data
    global multiUSVInitInfo
    global targer_location
    global Path_usv
    plt.ioff()  # 关闭交互模式
    while plotting:
        plt.clf()  # 清除之前的图表
        if len(targer_location) == 0 or len(multiUSVInitInfo) == 0:
            continue
        # 目标点
        # plt.scatter([x for x, _ in Task_data], [y for _, y in Task_data], color="blue")
        # for i, (x, y) in enumerate(Task_data):
        #     plt.text(x, y, f"Task{i}", color="blue", fontsize=8)
        #     circle = patches.Circle((x, y), 50, fill=False, edgecolor="blue")
        #     plt.gca().add_patch(circle)

        # 障碍物
        plt.scatter(
            [x for x, _, _ in Obs_data], [y for _, y, _ in Obs_data], color="green"
        )
        for i, (x, y, r) in enumerate(Obs_data):
            plt.text(x, y, f"Obs{i+1}", color="green", fontsize=8)
            circle = patches.Circle((x, y), r, fill=False, edgecolor="green")
            plt.gca().add_patch(circle)

        # 每次的航点
        plt.scatter(
            [x for x, _ in targer_location[0]],
            [y for _, y in targer_location[0]],
            color="blue",
        )

        # 绘制箭头
        for i, data in enumerate(targer_location[1]):
            dx = data[0] - multiUSVInitInfo[i].dbInitX
            dy = data[1] - multiUSVInitInfo[i].dbInitY
            plt.arrow(
                multiUSVInitInfo[i].dbInitX,
                multiUSVInitInfo[i].dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="pink",
                ec="pink",
            )
            path_x, path_y = zip(*Path_usv[i])
            path_x = [x - 200 for x in path_x]
            path_y = [y - 400 for y in path_y]
            plt.plot(path_x, path_y, "g--")
        # plt.scatter(
        #     [x for x, _ in targer_location[2]],
        #     [y for _, y in targer_location[2]],
        #     color="green",
        # )
        # plt.scatter(
        #     [x for x, _ in targer_location[3]],
        #     [y for _, y in targer_location[3]],
        #     color="purple",
        # )

        # USV位置
        plt.scatter([x for x, _ in USV_data], [y for _, y in USV_data], color="red")
        for i, (x, y) in enumerate(USV_data):
            plt.text(x, y, f"USV{i+1}", color="red", fontsize=8)
            circle = patches.Circle((x, y), 50, fill=False, edgecolor="red")
            plt.gca().add_patch(circle)

        for i, updateUSVControl in enumerate(multiUSVInitInfo):
            dx = updateUSVControl.fInitVel * np.cos(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            dy = updateUSVControl.fInitVel * np.sin(
                np.radians(90 - updateUSVControl.fInitYaw)
            )
            plt.arrow(
                updateUSVControl.dbInitX,
                updateUSVControl.dbInitY,
                dx,
                dy,
                head_width=10,
                head_length=10,
                fc="blue",
                ec="blue",
            )

        # 添加图例
        plt.legend()
        # 强制设置坐标轴范围

        plt.xlabel("X")
        plt.ylabel("Y")
        plt.axis("equal")  # 确保x和y轴的刻度一致，使得圆形看起来是圆的
        plt.pause(0.01)  # 重新绘制图表
    plt.close("all")


def main_function():
    global plotting
    global Task_data
    global Obs_data
    global USV_data
    global multiUSVInitInfo
    global targer_location
    global START
    global GOAL
    global Path_usv
    os.add_dll_directory(os.path.join(os.getcwd(), "./dll"))
    cmultilUSV = MultiUSVModel(
        dll_path=r"./dll/usvmodel.dll",
        dll_deps=[os.path.join(r"./dll", dll_i) for dll_i in os.listdir(r"./dll")],
    )

    # 态势信息
    curOBSFusion_data = ctypes.POINTER(InputOBSStruct)()  # 指向InputOBSStruct类型的指针
    curOBSFusion_size = ctypes.c_int()  # 障碍物数量
    # 导航信息
    multiUSVStatus_data = ctypes.POINTER(USVOutPutStructType)()
    multiUSVStatus_size = ctypes.c_int()

    isEnd_int = ctypes.c_int()
    # 任务航路
    taskPath_data = ctypes.POINTER(SFormationTaskInfo)()
    taskPath_size = ctypes.c_int()
    # 构型信息
    referInfo_data = ctypes.POINTER(referUSVInfo)()
    referInfo_size = ctypes.c_int()
    # 整个任务迭代次数
    iterNum = 0
    # 控制模式，1 - 航速航向；2 - 航路，建议使用航速航向
    flagModel = 1
    # 算法更新后清零
    algIterNum = 0
    save_time = 0
    # 初始化
    cmultilUSV.multiInitialize(flagModel)

    # 空指令
    # 更新控制指令
    cmultilUSV.updateMultiUSVControl(
        (USVInitStructType * len(multiUSVInitInfo))(*multiUSVInitInfo),
        len(multiUSVInitInfo),
        iterNum,
        flagModel,
    )
    # 获取导航信息和态势信息
    cmultilUSV.getMultiNaviObsInfo(
        iterNum,
        ctypes.byref(multiUSVStatus_data),
        ctypes.byref(multiUSVStatus_size),
        ctypes.byref(curOBSFusion_data),
        ctypes.byref(curOBSFusion_size),
    )
    # 获取任务信息
    rcvFlag = cmultilUSV.getTaskInfo(
        ctypes.byref(isEnd_int),
        ctypes.byref(taskPath_data),
        ctypes.byref(taskPath_size),
        ctypes.byref(referInfo_data),
        ctypes.byref(referInfo_size),
        iterNum,
    )
    # 仿真环境初始化
    _ = Get_USV_data(
        taskPath_data,
        taskPath_size,
        referInfo_data,
        referInfo_size,
        curOBSFusion_data,
        curOBSFusion_size,
        multiUSVStatus_data,
        multiUSVStatus_size,
    )

    controller = [
        USVLOSController(delta, kp, kd, max_angular_velocity, max_angular_acceleration)
        for _ in range(len(Refe_data))
    ]
    try:
        for i in range(1000 * 20):
            # 更新控制指令
            cmultilUSV.updateMultiUSVControl(
                (USVInitStructType * len(multiUSVInitInfo))(*multiUSVInitInfo),
                len(multiUSVInitInfo),
                iterNum,
                flagModel,
            )
            # 获取导航信息和态势信息
            cmultilUSV.getMultiNaviObsInfo(
                iterNum,
                ctypes.byref(multiUSVStatus_data),
                ctypes.byref(multiUSVStatus_size),
                ctypes.byref(curOBSFusion_data),
                ctypes.byref(curOBSFusion_size),
            )
            # 获取任务信息
            rcvFlag = cmultilUSV.getTaskInfo(
                ctypes.byref(isEnd_int),
                ctypes.byref(taskPath_data),
                ctypes.byref(taskPath_size),
                ctypes.byref(referInfo_data),
                ctypes.byref(referInfo_size),
                iterNum,
            )
            # 触发任务
            if 1 == rcvFlag:
                if 1 == isEnd_int.value:  # 到达最后一个任务点，不再发送任务
                    cmultilUSV.taskEnd(iterNum, flagModel)
                    break  # 跳出算法循环
                algIterNum = 0  # 新任务，次数清零
            # 算法更新控制指令
            multiUSVInitInfo = algorithmImpl(
                algIterNum,
                taskPath_data,
                taskPath_size,
                referInfo_data,
                referInfo_size,
                curOBSFusion_data,
                curOBSFusion_size,
                multiUSVStatus_data,
                multiUSVStatus_size,
                controller,
            )

            # 迭代次数更新
            iterNum += 1
            algIterNum += 1
            if is_done(USV_data, targer_location, 10):
                # 线程终止
                plotting = False
                save_time += 1
                if save_time > 100:
                    print("任务完成")
                    break
            else:
                save_time = 0
        # 当主线程的任务完成后，更新标志以停止绘图线程的循环
    except KeyboardInterrupt:
        plotting = False
        # 等待绘图线程结束
        # plot_thread.join()


if __name__ == "__main__":
    # 创建并启动绘图线程
    # main_function()
    main_thread = threading.Thread(target=main_function)
    main_thread.start()
    plot_graph()

# 把文件data改名为result_num
if os.path.exists("dataRecord"):
    # os.rename("dataRecord", "result_" + str(pathnum))
    os.rename("dataRecord", "result_conflict")

# if clean_flag:
#     clear_dataRecord_folder()
